#include "GpsThread.h"
#include "Beagle.h"

GpsThread::GpsThread(TinyGPSPlus* gps){
    _gps = gps;

    _lastLat = 0; //whole degrees
    _lastLon = 0; //whole degrees
    _lastSpeed = 0; //100ths of a knot
    _lastCourse = 0; //100ths of a degree
}

void GpsThread::run(){
    digitalWrite(38, HIGH);
    //Serial.println("g");

    bool check = true;

    while(check){
        if(Serial1.available() > 0){
            char c = Serial1.read();
            //Serial.print(c);
            bool encoded = _gps->encode(c);
            if(encoded){
                //Serial.println("GPS: encoded NMEA sentence");
                //if(_gps->location.isValid() && _gps->speed.isValid() && _gps->course.isValid()){
                    int32_t lat = _gps->location.rawLat();
                    int32_t lon = _gps->location.rawLng();
                    int32_t speed = _gps->speed.value();
                    int32_t course = _gps->course.value();
                    /*Serial.println(_gps->location.rawLat());
                    Serial.println(_gps->location.rawLng());
                    Serial.println(_gps->location.lat());
                    Serial.println(_gps->location.lng());*/

                    //if((lat != _lastLat) || (lon != _lastLon) || (speed != _lastSpeed) || (course != _lastCourse)){
                        Beagle::sendGpsSentence(lat, lon, speed, course);
                    //}

                    check = false;
                //}
            }
        }
    }

    digitalWrite(38, LOW);
}
